#include "histograma.h"

float valoresHistograma[24];
int indexMaxHist=0;
float posicionLinea=0;

extern uint32_t Luminosidad_Escalada_Frontal[];
extern uint32_t Luminosidad_Escalada_Trasera[];

void reiniciar_histograma(void)
{
		int i;
    for (i = 0; i < 24; i++) {
        valoresHistograma[i] = (float)1/24;
    }
}

float getPosicionLinea(int32_t bias)
{
    int32_t medidaPonderada[24];
    int32_t mediaValores = 0;
    float histogramaTotal = 0;
    float expandedHistogram[24];
	float probZonaLinea;
	int i;

    for (i = 0; i < 24; i++) {

        if ((i >= 4) && (i <= 19)) {
            medidaPonderada[i] = (int32_t)((Luminosidad_Escalada_Frontal[i] + Luminosidad_Escalada_Trasera[i-4])/2);
        }else {
            medidaPonderada[i] = Luminosidad_Escalada_Frontal[i];
        }

        //Contribuir para la media
        mediaValores += medidaPonderada[i];

        //Vaciamos el vector de histograma expandido
        expandedHistogram[i] = 0;
    }

    //Dividimos para obtener la media
    mediaValores /= 24;

    //Aplicamos incertidumbre de movimiento
    for (i = 0; i < 24; i++) {

        if (i > 1) {
            expandedHistogram[i - 2] += valoresHistograma[i]*0.083;
        }

        if (i > 0) {
            expandedHistogram[i - 1] += valoresHistograma[i]*0.167;
        }

        expandedHistogram[i] += valoresHistograma[i]*0.5;

        if (i < 23) {
            expandedHistogram[i + 1] += valoresHistograma[i]*0.167;
        }

        if (i < 22) {
            expandedHistogram[i + 2] += valoresHistograma[i]*0.083;
        }

    }

    //Multiplicamos por probabilidad	y anadimos bias si lo hay
    for (i = 0; i < 24; i++) {
        float probabilidad = generarConfianza(medidaPonderada, i, 2500);
        
		valoresHistograma[i] = expandedHistogram[i] * probabilidad;
		
		if (bias==1 && i>=12)
		{
			valoresHistograma[i] = valoresHistograma[i] * MULTIPLICADOR_BIAS;
		}
		if (bias==-1 && i<12)
		{
			valoresHistograma[i] = valoresHistograma[i] * MULTIPLICADOR_BIAS;
		}
        histogramaTotal += valoresHistograma[i];
    }

    //Normalizamos las probabilidades
    indexMaxHist = 0;
    for (i = 0; i < 24; i++ ){
        valoresHistograma[i] /= histogramaTotal;

        if (valoresHistograma[i] > valoresHistograma[indexMaxHist]) {
            indexMaxHist = i;
        }
    }

    //Calculamos la media
    posicionLinea = 0;
	probZonaLinea = 0;

    for (i = -2; i <= 2; i++) {
		
		if ((indexMaxHist + i) < 0) continue;
		if ((indexMaxHist + i) >= 24) break;

		posicionLinea += valoresHistograma[indexMaxHist + i] * (indexMaxHist + i);
		probZonaLinea += valoresHistograma[indexMaxHist + i];
	}

	posicionLinea /= probZonaLinea;

    return posicionLinea;
}

float* getHistograma()
{
    return valoresHistograma;
}

float generarConfianza(int32_t *valoresBarras, int indice, int32_t media)
{
    float confianza = (float)(media-valoresBarras[indice]);

    if (indice > 0) {
        confianza +=  (float)(media-valoresBarras[indice-1])/2;
    }

    if (indice < 24) {
        confianza +=  (float)(media-valoresBarras[indice+1])/2;
    }

    return (confianza > (float)0.1) ? confianza :(float)0.1;
}
